#ifndef FRAME_H
#define FRAME_H

#include <memory>
#include <utility>
#include <vector>
#include <ostream>

#include <Eigen/Dense>
#include "../Calibration.h"


namespace Estimator
{
using namespace Eigen;

class Frame
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    typedef std::pair<size_t /*id for landmark*/, std::array<double,2> /*pixel observation*/> LandMarkObservation;

    Frame() = delete;
    virtual ~Frame() = default;

    Frame(size_t id, const Matrix<double,3,3, RowMajor>& rotation, const Vector3d& trans):
             mRotationMatrix(rotation),mTranslationVec(trans),mId(id){}

    void SetPose(const Matrix<double,3,3, RowMajor>& rotation, const Vector3d& trans )
    {
      mRotationMatrix = rotation;
      mTranslationVec = trans;
    }

    const size_t& GetFrameId() const { return mId;}

    Eigen::Matrix<double,3,3, RowMajor> mRotationMatrix; //= Eigen::Matrix<double,3,3, Eigen::RowMajor>::Constant(3,3,std::nan(""));
    Vector3d mTranslationVec = Eigen::Vector3d::Constant(std::nan(""));

    template<class... Args> 
    void ConstructObservationsToVector(Args&&... args){ mvLandMarkObservations_.emplace_back(args...); }

    friend std::ostream& operator<<(std::ostream& os, const Frame& timer);

    // simply use vector for each frame. cause no need for sorting this. Pixel observations should be normalized 
    // according to the inner calibration parameter: 
    // | u_c |          | u |
    // | v_c | = K^(-1) | v |
    // |  1  |          | 1 |
    std::vector<LandMarkObservation> mvLandMarkObservations_;
    // std::vector<Vector3d>
    // std::vector<

  private:
    const size_t          mId;

};
typedef std::shared_ptr<Frame> FramePtr;

inline std::ostream& operator << (std::ostream& os, const Frame& frame)
{
    // os << timer.timer_name_ << " takes " << std::chrono::duration_cast<std::chrono::microseconds>(timer.end_ - timer.start_).count() << "us. ";

    os << "Rotation: " << std::endl << frame.mRotationMatrix << std::endl;
    os << "Translation: " << std::endl << frame.mTranslationVec << std::endl;
    os << "The total number of observations: " << frame.mvLandMarkObservations_.size() << std::endl;
    for(auto observation : frame.mvLandMarkObservations_)
    {
      os << observation.first << " " << observation.second[0] << " " << observation.second[1] << std::endl;
    }

    return os;
}

}

#endif // FRAME_H